/*
 * @Description: 
 * @FilePath: /LQ_TC26xB_LIBtasking/src/AppSw/Tricore/MY/API/MPU9250.c
 * @Author: Wei Zhou
 * @Github: https://github.com/zromyk
 * @Date: 1970-01-01 08:00:00
 * @LastEditors: Wei Zhou
 * @LastEditTime: 2020-05-22 21:00:17
 * @Copyright: Copyright © 2017 muyiro. All rights reserved.
 */ 

#include "dev_mpu9250.h"

MPUNode mpu = 
{
    .elem.kp    = 0.4f,     /*比例增益*/
    .elem.ki    = 0.001f,   /*积分增益*/
    .elem.exInt = 0.0f,
    .elem.eyInt = 0.0f,
    .elem.ezInt = 0.0f,     /*积分误差累计*/

    .elem.q0 = 1.0f,  /*四元数*/
    .elem.q1 = 0.0f,
    .elem.q2 = 0.0f,
    .elem.q3 = 0.0f
};

// 定义MPU9250内部地址
#define MPU9250_DEV_ADDR        0x68    //IIC写入时的地址字节数据，+1为读取

#define SMPLRT_DIV              0x19    //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG                  0x1A    //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG             0x1B    //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG            0x1C    //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_CONFIG2           0X1D    //加速度计低通滤波器 0x06 5hz
#define INT_PIN_CFG             0x37    //设置9250辅助I2C为直通模式寄存器
#define ACCEL_XOUT_H            0x3B
#define ACCEL_XOUT_L            0x3C
#define ACCEL_YOUT_H            0x3D
#define ACCEL_YOUT_L            0x3E
#define ACCEL_ZOUT_H            0x3F
#define ACCEL_ZOUT_L            0x40
#define TEMP_OUT_H              0X41    //温度计输出数据
#define TEMP_OUT_L              0X42
#define GYRO_XOUT_H             0x43
#define GYRO_XOUT_L             0x44    
#define GYRO_YOUT_H             0x45
#define GYRO_YOUT_L             0x46
#define GYRO_ZOUT_H             0x47
#define GYRO_ZOUT_L             0x48
#define USER_CTRL               0x6A    //关闭9250对辅助I2C设备的控制,用户配置当为0X10时使用SPI模式
#define PWR_MGMT_1              0x6B    //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I                0x75    //器件ID MPU9250默认ID为0X71

typedef enum IIC    //DAC模块
{
    IIC,
    SCCB
} IIC_type;

//内部数据定义
uint8 IIC_ad_main; //器件从地址
uint8 IIC_ad_sub;  //器件子地址
uint8 *IIC_buf;    //发送|接收数据缓冲区
uint8 IIC_num;     //发送|接收数据个数

#define ack 1      //主应答
#define no_ack 0   //从应答

// 模拟IIC延时，如果IIC通讯失败可以尝试增加j的值
static void simiic_delay(void)
{
    //j=10通讯速率大约为100K （内核频率40M）
    //j=0 通讯速率大约为140K （内核频率40M）
    uint16_t j=100;
    while(j--);
}

// 产生IIC起始信号
// 内部使用，用户无需调用
static void iic_start(void)
{
    MPU9250_SDA_OUT();
    MPU9250_SCL_H();
    MPU9250_SDA_H();
    simiic_delay();
    MPU9250_SDA_L();
    simiic_delay();
    MPU9250_SCL_L();
}

// 产生IIC停止信号
// 内部使用，用户无需调用
static void iic_stop(void)
{
    MPU9250_SDA_OUT();
    MPU9250_SCL_L();
    MPU9250_SDA_L();
    simiic_delay();
    MPU9250_SCL_H();
    simiic_delay();
    MPU9250_SDA_H();
    simiic_delay();
}

// 产生主应答信号(包含ack:SDA=0和no_ack:SDA=1)
// 内部使用，用户无需调用
static void iic_send_ack(uint8 ack_dat)
{
    MPU9250_SDA_OUT();
    MPU9250_SCL_L();
    simiic_delay();
    if(ack_dat) MPU9250_SDA_L(); // 产生应答
    else        MPU9250_SDA_H(); // 不产生应答

    simiic_delay();
    MPU9250_SCL_H();
    simiic_delay();
    MPU9250_SCL_L();
    simiic_delay();
}

// 等待应答信号到来
// 返回值： 1，接收应答失败
//          0，接收应答成功
// 内部使用，用户无需调用
static uint8 iic_wait_ack(void)
{
    uint8 err_time = 0;
    
    MPU9250_SDA_IN();
    MPU9250_SCL_L();
    simiic_delay();
    MPU9250_SCL_H();
    simiic_delay();

    while (MPU9250_READ_SDA)
    {
        err_time++;
        if (err_time > 250)
        {
            MPU9250_SDA_OUT();
            MPU9250_SCL_L();
            return 1;
        }
    }
    
    MPU9250_SDA_OUT();
    MPU9250_SCL_L();
    simiic_delay();
    return 0;
}

// IIC发送一个字节
// 发送c(可以是数据也可是地址)，送完后接收从应答
// 不考虑从应答位
// 内部使用，用户无需调用
static void iic_send_byte(uint8 c)
{
    uint8 i = 8;
    MPU9250_SDA_OUT();
    MPU9250_SCL_L();
    while(i--)
    {
        if(c & 0x80)    MPU9250_SDA_H(); // SDA 输出数据
        else            MPU9250_SDA_L();
        c <<= 1;
        simiic_delay();
        MPU9250_SCL_H();                //SCL 拉高，采集信号
        simiic_delay();
        MPU9250_SCL_L();                //SCL 时钟线拉低
        simiic_delay();
    }
    iic_wait_ack();
}

// IIC读一个字节
// ack_x = 1，产生应答；ack_x = 0，不产生应答
// 内部使用，用户无需调用
static uint8 iic_read_byte(uint8 ack_x)
{
    uint8 i, c;
    c=0;
    MPU9250_SCL_L();
    simiic_delay();
    MPU9250_SDA_H();
    MPU9250_SDA_IN();
    for(i=0;i<8;i++)
    {
        MPU9250_SCL_L();         //置时钟线为低，准备接收数据位
        simiic_delay();
        MPU9250_SCL_H();         //置时钟线为高，使数据线上数据有效
        simiic_delay();
        c <<= 1;
        if(MPU9250_READ_SDA) c++;   //读数据位，将接收的数据存c
        simiic_delay();
    }
    MPU9250_SDA_OUT();
    MPU9250_SCL_L();
    simiic_delay();
    iic_send_ack(ack_x);
    return c;
}

/**
 * @description: 模拟IIC写数据到设备寄存器函数
 * @param      dev_add         设备地址(低七位地址)
 * @param      reg             寄存器地址
 * @param      dat             写入的数据
 */
static void simiic_write_reg(uint8 dev_add, uint8 reg, uint8 dat)
{
    iic_start();
    iic_send_byte( (dev_add<<1) | 0x00);   //发送器件地址加写位
    iic_send_byte( reg );                  //发送从机寄存器地址
    iic_send_byte( dat );                  //发送需要写入的数据
    iic_stop();
}

/**
 * @description: 模拟IIC从设备寄存器读取数据
 * @param      dev_add         设备地址(低七位地址)
 * @param      reg             寄存器地址
 * @param      type            选择通信方式是IIC  还是 SCCB
 * @return     uint8         返回寄存器的数据
 */
uint8 simiic_read_reg(uint8 dev_add, uint8 reg, IIC_type type)
{
    uint8 dat;
    iic_start();
    iic_send_byte( (dev_add<<1) | 0x00);  //发送器件地址加写位
    iic_send_byte( reg );                 //发送从机寄存器地址
    if(type == SCCB)iic_stop();
    
    iic_start();
    iic_send_byte( (dev_add<<1) | 0x01);  //发送器件地址加读位
    dat = iic_read_byte(no_ack);                  //读取数据
    iic_stop();
    
    return dat;
}

/**
 * @description: 模拟IIC读取多字节数据
 * @param      dev_add         设备地址(低七位地址)
 * @param      reg             寄存器地址
 * @param      dat_add         数据保存的地址指针
 * @param      num             读取字节数量
 * @param      type            选择通信方式是IIC  还是 SCCB
 */
static void simiic_read_regs(uint8 dev_add, uint8 reg, uint8 *dat_add, uint8 num, IIC_type type)
{
    iic_start();
    iic_send_byte( (dev_add<<1) | 0x00);  //发送器件地址加写位
    iic_send_byte( reg );                 //发送从机寄存器地址
    if(type == SCCB)iic_stop();
    
    iic_start();
    iic_send_byte( (dev_add<<1) | 0x01);  //发送器件地址加读位
    while(--num)
    {
        *dat_add = iic_read_byte(ack); //读取数据
        dat_add++;
    }
    *dat_add = iic_read_byte(no_ack); //读取数据
    iic_stop();
}

/**
 * @description: 初始化MPU9250
 * 调用该函数前，请先调用模拟IIC的初始化
 * 
 *  ACCEL_CONFIG
 *      0x00: 2g
 *      0x08: 4g
 *      0x10: 8g
 *      0x18: 16g
 *  GYRO_CONFIG:
 *      0x00: 250deg/s
 *      0x08: 500deg/s
 *      0x10: 1000deg/s 
 *      0x18: 2000deg/s
 */
#define ACC_SENSITIVITY     (4095.875f) // 32767.0f / 8.0f
#define GYRO_SENSITIVITY    (16.388f)   // 32767.0f / 2000.0f

// MPU9250引脚初始化
void mpu9250_pin_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
	MPU9250_RCC_APBx_CMD(MPU9250_RCC_APBx, ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = MPU9250_SCL_GPIO_Pin | MPU9250_SDA_GPIO_Pin;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MPU9250_GPIOx, &GPIO_InitStructure);
    //MPU9250_SCL_H();
    //MPU9250_SDA_H();
}

// MPU9250初始化
void mpu9250_init(void)
{
    mpu9250_pin_init();
    
    simiic_write_reg(MPU9250_DEV_ADDR, PWR_MGMT_1, 0x00);      // 解除休眠状态
    simiic_write_reg(MPU9250_DEV_ADDR, SMPLRT_DIV, 0x10);      // 采样率, 8kHz / (1 + SMPLRT_DIV)
    simiic_write_reg(MPU9250_DEV_ADDR, CONFIG, 0x02);
    simiic_write_reg(MPU9250_DEV_ADDR, ACCEL_CONFIG, 0x10);    
    //simiic_write_reg(MPU9250_DEV_ADDR, ACCEL_CONFIG2, 0x06);    //加速度采样频率460HZ
    simiic_write_reg(MPU9250_DEV_ADDR, GYRO_CONFIG, 0x18);
    simiic_write_reg(MPU9250_DEV_ADDR, USER_CTRL, 0x00);
    simiic_write_reg(MPU9250_DEV_ADDR, INT_PIN_CFG, 0x02);
    
//    OledPrintValueI(0,0,simiic_read_reg(MPU9250_DEV_ADDR, PWR_MGMT_1, SCCB));
//    OledPrintValueI(0,1,simiic_read_reg(MPU9250_DEV_ADDR, SMPLRT_DIV, SCCB));
//    OledPrintValueI(0,2,simiic_read_reg(MPU9250_DEV_ADDR, CONFIG, SCCB));
//    OledPrintValueI(0,3,simiic_read_reg(MPU9250_DEV_ADDR, ACCEL_CONFIG, SCCB));
//    //OledPrintValueI(0,4,simiic_read_reg(MPU9250_DEV_ADDR, ACCEL_CONFIG2, SCCB));
//    OledPrintValueI(0,5,simiic_read_reg(MPU9250_DEV_ADDR, GYRO_CONFIG, SCCB));
//    OledPrintValueI(0,6,simiic_read_reg(MPU9250_DEV_ADDR, USER_CTRL, SCCB));
//    OledPrintValueI(0,7,simiic_read_reg(MPU9250_DEV_ADDR, INT_PIN_CFG, SCCB));
//    while(1);
}

/**
 * @description: 采集一次加速度计的值
 */
static void GetAccData(MPUNode *mpu)
{
    uint8 dat[6];
    
    simiic_read_regs(MPU9250_DEV_ADDR, ACCEL_XOUT_H, dat, 6, IIC);  
    mpu->acc.x = (int16_t)((uint16_t)dat[0]<<8 | dat[1]);
    mpu->acc.y = (int16_t)((uint16_t)dat[2]<<8 | dat[3]);
    mpu->acc.z = (int16_t)((uint16_t)dat[4]<<8 | dat[5]);

    mpu->accRaw.x  = mpu->acc.x; // 向前的加速度
    mpu->accRaw.y  = mpu->acc.y; // 左右的加速度
    mpu->accRaw.z  = mpu->acc.z; // 向下的加速度
}

/**
 * @description: 采集一次陀螺仪的值
 */
static void GetGyroData(MPUNode *mpu)
{
    uint8 dat[6];
    
    simiic_read_regs(MPU9250_DEV_ADDR, GYRO_XOUT_H, dat, 6, IIC);  
    mpu->gyro.x = (int16_t)((uint16_t)dat[0]<<8 | dat[1]);
    mpu->gyro.y = (int16_t)((uint16_t)dat[2]<<8 | dat[3]);
    mpu->gyro.z = (int16_t)((uint16_t)dat[4]<<8 | dat[5]);
    
    mpu->gyro.x = mpu->gyro.x + (64);
    mpu->gyro.y = mpu->gyro.y + (-35);
    mpu->gyro.z = mpu->gyro.z + (5);
    
    mpu->gyro.x = _abs(mpu->gyro.x) < 25 ? 0 : mpu->gyro.x;
    mpu->gyro.y = _abs(mpu->gyro.y) < 25 ? 0 : mpu->gyro.y;
    mpu->gyro.z = _abs(mpu->gyro.z) < 25 ? 0 : mpu->gyro.z; 

    mpu->gyroRaw.x = mpu->gyro.x / GYRO_SENSITIVITY;// 翻滚角角速度
    mpu->gyroRaw.y = mpu->gyro.y / GYRO_SENSITIVITY;// 俯仰角角速度
    mpu->gyroRaw.z = mpu->gyro.z / GYRO_SENSITIVITY;// 偏航角角速度
}

/**
 * @description: 快速开平方求倒 ( Fast inverse square-root )
 * http://en.wikipedia.org/wiki/Fast_inverse_square_root
 */
static float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * @description: 计算旋转矩阵
 */
static void imuComputeRotationMatrix(ElemNode *elem)
{
    float q1q1 = elem->q1 * elem->q1;
    float q2q2 = elem->q2 * elem->q2;
    float q3q3 = elem->q3 * elem->q3;

    float q0q1 = elem->q0 * elem->q1;
    float q0q2 = elem->q0 * elem->q2;
    float q0q3 = elem->q0 * elem->q3;
    float q1q2 = elem->q1 * elem->q2;
    float q1q3 = elem->q1 * elem->q3;
    float q2q3 = elem->q2 * elem->q3;

    elem->rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    elem->rMat[0][1] = 2.0f * (q1q2 + -q0q3);
    elem->rMat[0][2] = 2.0f * (q1q3 - -q0q2);

    elem->rMat[1][0] = 2.0f * (q1q2 - -q0q3);
    elem->rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    elem->rMat[1][2] = 2.0f * (q2q3 + -q0q1);

    elem->rMat[2][0] = 2.0f * (q1q3 + -q0q2);
    elem->rMat[2][1] = 2.0f * (q2q3 - -q0q1);
    elem->rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}

/**
 * @description: 四元素求解欧拉角
 * @param {type} 
 * @return: 
 */

#define DEG2RAD     0.017453293f    /* 度转弧度 π/180 */
#define RAD2DEG     57.29578f       /* 弧度转度 180/π */

static void imuUpdate(MPUNode *mpu, float dt)
{
    float normalise;
    float ex, ey, ez;
    float halfT = 0.5f * dt;

    mpu->gyroRaw.x = mpu->gyroRaw.x * DEG2RAD;  /* 度转弧度 */
    mpu->gyroRaw.y = mpu->gyroRaw.y * DEG2RAD;
    mpu->gyroRaw.z = mpu->gyroRaw.z * DEG2RAD;

    /* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
    if((mpu->accRaw.x != 0.0f) || (mpu->accRaw.y != 0.0f) || (mpu->accRaw.z != 0.0f))
    {
        /*单位化加速计测量值*/
        normalise = invSqrt(mpu->accRaw.x * mpu->accRaw.x + mpu->accRaw.y * mpu->accRaw.y + mpu->accRaw.z * mpu->accRaw.z);
        mpu->accRaw.x *= normalise;
        mpu->accRaw.y *= normalise;
        mpu->accRaw.z *= normalise;

        /*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
        ex = (mpu->accRaw.y * mpu->elem.rMat[2][2] - mpu->accRaw.z * mpu->elem.rMat[2][1]);
        ey = (mpu->accRaw.z * mpu->elem.rMat[2][0] - mpu->accRaw.x * mpu->elem.rMat[2][2]);
        ez = (mpu->accRaw.x * mpu->elem.rMat[2][1] - mpu->accRaw.y * mpu->elem.rMat[2][0]);

        /*误差累计，与积分常数相乘*/
        mpu->elem.exInt += mpu->elem.ki * ex * dt ;
        mpu->elem.eyInt += mpu->elem.ki * ey * dt ;
        mpu->elem.ezInt += mpu->elem.ki * ez * dt ;

        /*用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量*/
        mpu->gyroRaw.x += mpu->elem.kp * ex + mpu->elem.exInt;
        mpu->gyroRaw.y += mpu->elem.kp * ey + mpu->elem.eyInt;
        mpu->gyroRaw.z += mpu->elem.kp * ez + mpu->elem.ezInt;
    }
    /* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
    float q0Last = mpu->elem.q0;
    float q1Last = mpu->elem.q1;
    float q2Last = mpu->elem.q2;
    float q3Last = mpu->elem.q3;
    mpu->elem.q0 += (-q1Last * mpu->gyroRaw.x - q2Last * mpu->gyroRaw.y - q3Last * mpu->gyroRaw.z) * halfT;
    mpu->elem.q1 += ( q0Last * mpu->gyroRaw.x + q2Last * mpu->gyroRaw.z - q3Last * mpu->gyroRaw.y) * halfT;
    mpu->elem.q2 += ( q0Last * mpu->gyroRaw.y - q1Last * mpu->gyroRaw.z + q3Last * mpu->gyroRaw.x) * halfT;
    mpu->elem.q3 += ( q0Last * mpu->gyroRaw.z + q1Last * mpu->gyroRaw.y - q2Last * mpu->gyroRaw.x) * halfT;

    /*单位化四元数*/
    normalise = invSqrt(mpu->elem.q0 * mpu->elem.q0 + mpu->elem.q1 * mpu->elem.q1 + mpu->elem.q2 * mpu->elem.q2 + mpu->elem.q3 * mpu->elem.q3);
    mpu->elem.q0 *= normalise;
    mpu->elem.q1 *= normalise;
    mpu->elem.q2 *= normalise;
    mpu->elem.q3 *= normalise;

    imuComputeRotationMatrix(&mpu->elem);  /*计算旋转矩阵*/

    /*计算roll pitch yaw 欧拉角*/
    mpu->pitch.angle = asinf(mpu->elem.rMat[2][0]) * RAD2DEG;
    mpu->roll.angle  = atan2f(mpu->elem.rMat[2][1], mpu->elem.rMat[2][2]) * RAD2DEG;
    mpu->yaw.angle   = atan2f(mpu->elem.rMat[1][0], mpu->elem.rMat[0][0]) * RAD2DEG;
}

void MPU9250_GetInfo(void)
{
    GetAccData(&mpu);
    GetGyroData(&mpu);

    imuUpdate(&mpu, MPU_TIM_COUNT_CYCLE);
}
